#pragma once
#include <iomanip>
#include "common.h"
#include "drone.h"
#include "car.h"
#include "enu_converter.h"


/**
 * 从双无人车获取目标的enu坐标
 * @param car1_wgs84    车1的经纬度坐标
 * @param car2_wgs84    车2的经纬度坐标
 * @param flag          是否成功获取目标物体的enu坐标
 * @return              目标物体的enu坐标
 */
Vector3d get_obj_enu_from_car(VehicleWgs84 &car1_wgs84, VehicleWgs84 &car2_wgs84, bool& flag);

/**
 * 从无人机获取目标的enu坐标
 * @param drone_wgs84   无人机的经纬度坐标
 * @param camera_pos    相机相对无人机的位置
 * @param pixel         识别到目标物体的像素
 * @param drone_rot     无人机旋转矩阵
 * @param camera_rot    相机的旋转矩阵
 * @param flag          是否成功获取目标物体的enu坐标
 * @return              目标物体的enu坐标
 */
Vector3d get_obj_enu_from_drone(Vector3d &drone_wgs84, Vector3d &camera_pos,
                                Vector3d &pixel, Matrix3x3 &drone_rot, Matrix3x3 &camera_rot, bool& flag);

/**
 * 目标的enu坐标转换成wgs84经纬度
 * @param base_wgs84    基准点的经纬度
 * @param obj_enu       目标物体的enu坐标
 * @return              目标物体的经纬度坐标
 */
Vector3d obj_from_enu2wgs84(VehicleWgs84 &base_wgs84, Vector3d &obj_enu);

/**
 * 融合两个经纬度坐标
 * @param obj1      经纬度坐标1
 * @param obj2      经纬度坐标2
 * @param alpha     融合因子
 * @return          融合后的经纬度坐标
 */
Vector3d locate_fusion(Vector3d &obj1, Vector3d &obj2, double alpha = 0.5);
